#ifndef VIEW_POINT_SERVER_H
#define VIEW_POINT_SERVER_H

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <quadrotor_msgs/GoalSet.h>
#include <fstream>

#include <ros/package.h>

class ViewPointServer
{
public:
    enum SERVER_MODE {RECORD = 0, PUBLISH = 1};
    ViewPointServer(SERVER_MODE mode, ros::NodeHandle &nh);
    ~ViewPointServer();
    std::vector<geometry_msgs::Point> waypoints_;
    int cur_waypoint_index_;

private:
    void odomCallback(const nav_msgs::OdometryConstPtr &msg);
    void recordTriggerCallback (const geometry_msgs::PoseStampedConstPtr &msg);
    void goalPubTriggerCallback (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
    void saveWaypoints(const std::string &save_file_path);
    bool loadWaypoints(const std::string &save_file_path);
    void goalHandler(const ros::TimerEvent &event);
    void goalVisualization(const int& target_id);
    void allGoalVisualization();

    template<typename T>
    void readParam(ros::NodeHandle &node, std::string param_name, T &param_val, T default_val);

    ros::NodeHandle     nh_;
    SERVER_MODE         _mode;                    // 服务器模式
    nav_msgs::Odometry  current_odom_;            // 无人机当前odometry (callback)

    geometry_msgs::PoseWithCovarianceStamped goal_tmp;


    ros::Subscriber odom_sub_;                   // 订阅无人机当前odometry
    ros::Subscriber record_trigger_sub_;         // 订阅保存点位的触发信号
    ros::Subscriber goal_trigger_sub_;           // 订阅发布目标点位的触发信号
    ros::Publisher  goal_pub_;                   // 发布目标点位
    ros::Timer      goal_pub_timer_;             // 定时器，发布目标点位

    ros::Publisher  target_vis;                  // 发布目标点位的可视化

    double _nxt_goal_distance_threshold;
    double vis_spheral_size_;
    std::string save_file_path_;
    bool already_published_;
};

#endif // VIEW_POINT_SERVER_H